#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <turtlesim/msg/pose.hpp>



class TurtleTFBroadcaster:public rclcpp::Node{
public:
    explicit TurtleTFBroadcaster(std::string name):Node(name){
        turtle_name_=this->declare_parameter("turtlename","turtle1");
        std::string topic_name="/"+turtle_name_+"/pose";
        pose_sub_=this->create_subscription<turtlesim::msg::Pose>(topic_name,\
            10,std::bind(&TurtleTFBroadcaster::tf_callback,this,std::placeholders::_1));
        tf_bc_=std::make_shared<tf2_ros::TransformBroadcaster>(*this);
    }

    void tf_callback(const std::shared_ptr<turtlesim::msg::Pose> msg){
        // RCLCPP_INFO(this->get_logger(),"Get pose info,ready to send transform...");
        // 2
        geometry_msgs::msg::TransformStamped tf_obj;
        // 3: fill
        tf_obj.header.stamp=this->get_clock()->now();
        tf_obj.header.frame_id="world";
        tf_obj.child_frame_id=turtle_name_;

        // translation
        tf_obj.transform.translation.x=msg->x;
        tf_obj.transform.translation.y=msg->y;
        tf_obj.transform.translation.z=0.0;

        // rotation
        tf2::Quaternion q;
        q.setRPY(0.0,0.0,msg->theta);
        tf_obj.transform.rotation.x=q[0];
        tf_obj.transform.rotation.y=q[1];
        tf_obj.transform.rotation.z=q[2];
        tf_obj.transform.rotation.w=q[3];

        // 4:publish
        tf_bc_->sendTransform(tf_obj);
    }

private:
   // 1
   std::shared_ptr<tf2_ros::TransformBroadcaster> tf_bc_{nullptr};
   std::string turtle_name_{};
   rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_sub_{nullptr};
};

int main(int argc,char** argv){
    rclcpp::init(argc,argv);
    auto node=std::make_shared<TurtleTFBroadcaster>("turtle_tf_broadcaster");
    rclcpp::spin(node);
    rclcpp::shutdown();

    return 0;
}